// MCService.cpp : Defines the entry point for the application.
//

#include "framework.h"
#include "MCService.h"

#define MAX_LOADSTRING 100

// Global Variables:
HINSTANCE hInst;                                // current instance
WCHAR szTitle[MAX_LOADSTRING];                  // The title bar text
WCHAR szWindowClass[MAX_LOADSTRING];            // the main window class name
HWND hWnd;                                      // Main window descriptor
NOTIFYICONDATA nid = { sizeof(nid) };           // Notify icon data
bool trayIcon;
HANDLE logFile;
short m_State;                                  // Current service status
cv::Mat rawL,rawR,rawM;                         // Left/right/merged camera image
cv::VideoCapture camL,camR;                     // Left/right camera capture
bool captured;                                  // Frame is captured
int camL_id, camR_id;                           // Left/right camera ID
int view_cam = 0;                               // Left/right/merged cam view
int mode = 0;                                   // Mode (capturing/track calibration/motion calibration)
int dp = 20, minDist = 150, p1 = 50, p2 = 40, minR = 10, maxR = 50, alpha = 0, beta = 256;
int h_min, h_max, s_min, s_max, v_min, v_max;   // HSV range variables
std::vector<float> realPx, imgPx, realPy, imgPy, realPz, imgPz;
int realX, realY, realZ;
cv::Point3i new_pos, old_pos;
int max_realX = 200, max_realY = 200, max_realZ = 200;
float Mx, My, Mz;
int asp_delta = 50, angle_delta = 5;
int kernel_size = 7;
cv::Point cL, cR;                               // Left/right detected circle's center
cv::Vec3b COLOR_MIN_L(15, 124, 200);
cv::Vec3b COLOR_MAX_L(19, 198, 239);
cv::Vec3b COLOR_MIN_R(15, 124, 200);
cv::Vec3b COLOR_MAX_R(19, 198, 239);
const char* track_calibr_wnd = "Tracking Calibration";
const char* stereo_calibr_wnd = "Stereo Calibration";
const char* motion_calibr_wnd = "Motion Calibration";


// Forward declarations of functions included in this code module:

float eu_dist(cv::Point p1, cv::Point p2)
{
    return sqrtf((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
}


int APIENTRY wWinMain(_In_ HINSTANCE hInstance,
                     _In_opt_ HINSTANCE hPrevInstance,
                     _In_ LPWSTR    lpCmdLine,
                     _In_ int       nCmdShow)
{
    UNREFERENCED_PARAMETER(hPrevInstance);
    UNREFERENCED_PARAMETER(lpCmdLine);

    // TODO: Place code here.
    
    // Initialize global strings
    LoadStringW(hInstance, IDS_APP_TITLE, szTitle, MAX_LOADSTRING);
    LoadStringW(hInstance, IDC_MCSERVICE, szWindowClass, MAX_LOADSTRING);
    MyRegisterClass(hInstance);

    // Perform application initialization:
    if (!InitInstance (hInstance, nCmdShow))
    {
        return FALSE;
    }

    HACCEL hAccelTable = LoadAccelerators(hInstance, MAKEINTRESOURCE(IDC_MCSERVICE));

    MSG msg;

    // Main message loop:
    while (GetMessage(&msg, nullptr, 0, 0))
    {
        if (!TranslateAccelerator(msg.hwnd, hAccelTable, &msg))
        {
            TranslateMessage(&msg);
            DispatchMessage(&msg);
        }
    }

    return (int) msg.wParam;
}



//
//  FUNCTION: MyRegisterClass()
//
//  PURPOSE: Registers the window class.
//
ATOM MyRegisterClass(HINSTANCE hInstance)
{
    WNDCLASSEXW wcex{};

    wcex.cbSize = sizeof(WNDCLASSEX);

    wcex.style          = CS_HREDRAW | CS_VREDRAW;
    wcex.lpfnWndProc    = WndProc;
    wcex.cbClsExtra     = 0;
    wcex.cbWndExtra     = 0;
    wcex.hInstance      = hInstance;
    wcex.hIcon          = LoadIcon(hInstance, MAKEINTRESOURCE(IDI_STEREO));
    wcex.hCursor        = LoadCursor(nullptr, IDC_ARROW);
    wcex.hbrBackground  = (HBRUSH)(COLOR_WINDOW+1);
    wcex.lpszMenuName   = MAKEINTRESOURCEW(IDC_MCSERVICE);
    wcex.lpszClassName  = szWindowClass;
    wcex.hIconSm        = LoadIcon(wcex.hInstance, MAKEINTRESOURCE(IDI_STEREO));

    return RegisterClassExW(&wcex);
}

//
//   FUNCTION: InitInstance(HINSTANCE, int)
//
//   PURPOSE: Saves instance handle and creates main window
//
//   COMMENTS:
//
//        In this function, we save the instance handle in a global variable and
//        create and display the main program window.
//
BOOL InitInstance(HINSTANCE hInstance, int nCmdShow)
{
   hInst = hInstance; // Store instance handle in our global variable

   hWnd = CreateWindowW(szWindowClass, szTitle, WS_SYSMENU | WS_CAPTION,// WS_OVERLAPPEDWINDOW|WS_BORDER,
      CW_USEDEFAULT, 0, CW_USEDEFAULT, 0, nullptr, nullptr, hInstance, nullptr);

   if (!hWnd)
   {
      return FALSE;
   }
   m_State = STATE_INITIALISING;
   rawL = cv::Mat();
   rawR = cv::Mat();
   rawM = cv::Mat();
   new_pos = cv::Point3d(0, 0, 0);
   captured = false;
   camL_id = 2;
   camR_id = 1;
   camL = cv::VideoCapture();
   camR = cv::VideoCapture();
   if (InitializeCapture() == S_OK)
   {
        // TODO: indicate success 
   }
   logFile = CreateFileW(L"log.txt", FILE_APPEND_DATA, FILE_SHARE_READ, NULL, OPEN_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
   SendMsg("Log started\n");
   ShowWindow(hWnd, nCmdShow);
   UpdateWindow(hWnd);
   nid.hWnd = hWnd;
   nid.uFlags = NIF_ICON | NIF_TIP | NIF_MESSAGE;// | NIF_SHOWTIP | NIF_GUID;
   nid.hIcon = LoadIcon(hInst, MAKEINTRESOURCE(IDI_STEREO));
   nid.uCallbackMessage = WM_TRAYICON;
   wcscpy_s(nid.szTip, L"Motion tracking service");
   trayIcon = Shell_NotifyIcon(NIM_ADD, &nid);
   LoadConfig();
}

void LoadConfig()
{
    cv::FileStorage param_file = cv::FileStorage("params.xml", cv::FileStorage::READ);
    if (param_file.isOpened())
    {
        param_file["aspect_ratio"] >> asp_delta;
        param_file["angle_delta"] >> angle_delta;
        param_file["minDist"] >> minDist;
        param_file["dp"] >> dp;
        param_file["kernel_size"] >> kernel_size;
        param_file["p1"] >> p1;
        param_file["p2"] >> p2;
        param_file["minR"] >> minR;
        param_file["maxR"] >> maxR;
        param_file["alpha"] >> alpha;
        param_file["beta"] >> beta;
        param_file["COLOR_MIN_L"] >> COLOR_MIN_L;
        param_file["COLOR_MAX_L"] >> COLOR_MAX_L;
        param_file["COLOR_MIN_R"] >> COLOR_MIN_R;
        param_file["COLOR_MAX_R"] >> COLOR_MAX_R;
        SetColors(view_cam);
        UpdateHSVTrackbars();
        param_file["Mx"] >> Mx;
        param_file["My"] >> My;
        param_file["Mz"] >> Mz;
        if (cv::getWindowProperty(track_calibr_wnd, cv::WND_PROP_VISIBLE) > 0)
        {
            cv::setTrackbarPos("dp*10:", track_calibr_wnd, dp);
            cv::setTrackbarPos("Kernel size:", track_calibr_wnd, kernel_size);
            cv::setTrackbarPos("Min dist:", track_calibr_wnd, minDist);
            cv::setTrackbarPos("Canny thr:", track_calibr_wnd, p1);
            cv::setTrackbarPos("Accum thr:", track_calibr_wnd, p2);
            cv::setTrackbarPos("Min radius:", track_calibr_wnd, minR);
            cv::setTrackbarPos("Max radius:", track_calibr_wnd, maxR);
            cv::setTrackbarPos("Alpha:", track_calibr_wnd, alpha);
            cv::setTrackbarPos("Beta:", track_calibr_wnd, beta);

            //cv::setTrackbarPos("Hue min:", track_calibr_wnd, h_min);
            //cv::setTrackbarPos("Hue max:", track_calibr_wnd, h_max);
            //cv::setTrackbarPos("Sat min:", track_calibr_wnd, s_min);
            //cv::setTrackbarPos("Sat max:", track_calibr_wnd, s_max);
            //cv::setTrackbarPos("Val min:", track_calibr_wnd, v_min);
            //cv::setTrackbarPos("Val max:", track_calibr_wnd, v_max);
        }


    }
    param_file.release();
    SendMsg("Configuration loaded\n");
}

void SaveConfig()
{
    cv::FileStorage param_file = cv::FileStorage("params.xml", cv::FileStorage::WRITE);
    //        param_file.write("minDistance", minDist);
    param_file << "aspect_ratio" << asp_delta;
    param_file << "angle_delta" << angle_delta;
    param_file << "dp" << dp;
    param_file << "kernel_size" << kernel_size;
    param_file << "minDist" << minDist;
    param_file << "p1" << p1;
    param_file << "p2" << p2;
    param_file << "minR" << minR;
    param_file << "maxR" << maxR;
    GetColors(view_cam);
    param_file << "COLOR_MIN_L" << COLOR_MIN_L;
    param_file << "COLOR_MAX_L" << COLOR_MAX_L;
    param_file << "COLOR_MIN_R" << COLOR_MIN_R;
    param_file << "COLOR_MAX_R" << COLOR_MAX_R;
    param_file << "alpha" << alpha;
    param_file << "beta" << beta;

    param_file << "Mx" << Mx;
    param_file << "My" << My;
    param_file << "Mz" << Mz;

    param_file.release();

}

void SetCoeff()
{
    cv::Mat sol(1, 1, CV_32F);
    if (realPx.size() > 0)
    {
        cv::Mat real_x_mat(realPx.size(), 1, CV_32F, realPx.data());
        cv::Mat img_x_mat(imgPx.size(), 1, CV_32F, imgPx.data());
        cv::solve(real_x_mat, img_x_mat, sol, cv::DECOMP_QR);
        Mx = sol.at<float>(0, 0);
    }
    if (realPy.size() > 0)
    {
        cv::Mat real_y_mat(realPy.size(), 1, CV_32F, realPy.data());
        cv::Mat img_y_mat(imgPy.size(), 1, CV_32F, imgPy.data());
        cv::solve(real_y_mat, img_y_mat, sol, cv::DECOMP_QR);
        My = sol.at<float>(0, 0);
    }
    if (realPz.size() > 0)
    {
        cv::Mat real_z_mat(realPz.size(), 1, CV_32F, realPz.data());
        cv::Mat img_z_mat(imgPz.size(), 1, CV_32F, imgPz.data());
        cv::solve(real_z_mat, img_z_mat, sol, cv::DECOMP_QR);
        Mz = sol.at<float>(0, 0);
    }
}

LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
{
    switch (message)
    {
    case WM_COMMAND:
        {
            int wmId = LOWORD(wParam);
            HMENU hMenu = GetMenu(hWnd);
            // Parse the menu selections:
            switch (wmId)
            {

            case IDM_FILE_LOADCONFIG:
                LoadConfig();
                break;
            case IDM_FILE_SAVECONFIG:
                SaveConfig();
                break;
            case IDM_EXIT:
                DestroyWindow(hWnd);
                break;
            case IDM_OPERATE_RUN:
                StartCapturing();
                break;
            case IDM_OPERATE_STOP:
                AbortCapturing();
                break;
            case IDM_CALIBRATE_TRACKING:
                if (mode!=TRACKING_CALIBRATE_MODE)
                    CalibrateTracking();
                break;
            case IDM_CALIBRATE_MOTION:
                if (mode!=MOTION_CALIBRATE_MODE)
                    CalibrateMotion();
                break;
            case IDM_VIEW_LEFTCAM:
                SetViewCam(LEFT_CAMERA_VIEW);
                break;
            case IDM_VIEW_RIGHTCAM:
                SetViewCam(RIGHT_CAMERA_VIEW);
                break;
            case IDM_VIEW_MERGEDVIEW:
                SetViewCam(MERGED_CAMERA_VIEW);
                break;
            case IDM_ABOUT:
                DialogBox(hInst, MAKEINTRESOURCE(IDD_ABOUTBOX), hWnd, About);
                break;
            default:
                return DefWindowProc(hWnd, message, wParam, lParam);
            }
        }
        break;
    case WM_PAINT:
        {
            PAINTSTRUCT ps;
            HDC hdc = BeginPaint(hWnd, &ps);
            if ((m_State != STATE_INITIALISING) && (m_State != STATE_ERROR))
            {
//                RECT& rc = *(RECT*)ps.rcPaint;
                long    w = rawM.cols,// abs(ps.rcPaint.right - ps.rcPaint.left),
                        h = rawM.rows;// abs(ps.rcPaint.bottom - ps.rcPaint.top);
                if ((w * h) > 0)
                {
                    //Rectangle(hdc, 0, 0, w, h);
                    BITMAPINFO bmpInfo;
                    //size_t bytesPerPixel = rawL.elemSize();
                    memset(&bmpInfo, 0, sizeof(bmpInfo));
                    bmpInfo.bmiHeader.biSize = sizeof(BITMAPINFOHEADER);
                    bmpInfo.bmiHeader.biWidth = rawM.cols;
                    bmpInfo.bmiHeader.biHeight = rawM.rows;
                    bmpInfo.bmiHeader.biPlanes = 1;
                    bmpInfo.bmiHeader.biBitCount = (WORD)rawM.elemSize() * 8;
                    bmpInfo.bmiHeader.biCompression = BI_RGB;
                    bmpInfo.bmiHeader.biXPelsPerMeter = 100;
                    bmpInfo.bmiHeader.biYPelsPerMeter = 100;
                    StretchDIBits(hdc, 0, h, w, -h, 0, 0, rawM.cols, rawM.rows, rawM.data, &bmpInfo, DIB_RGB_COLORS, SRCCOPY);
                }
            }
            // TODO: Add any drawing code that uses hdc here...
            EndPaint(hWnd, &ps);
        }
        break;
    case WM_DESTROY:
    {
        if (trayIcon)
            trayIcon = Shell_NotifyIcon(NIM_DELETE, &nid);
        SendMsg("Log is closed\n");
        CloseHandle(logFile);
        PostQuitMessage(0);
    }
        break;
    case WM_TIMER:
        {
            CaptureFrame();
            if (mode == CAPTURE_MODE)
            {
                if (captured)
                {
                    old_pos = new_pos;
                    new_pos = cv::Point3i(cL.x, cL.y, (int)eu_dist(cL, cR));
                    cv::Point3i delta = new_pos - old_pos;
                    if ((cv::abs(delta.x) + cv::abs(delta.y) + cv::abs(delta.z)) > 2)
                    {
                        m_State = STATE_OBJ_MOVING;
                        SetIcon(IDI_STEREO_RED);
                    }
                    else
                    {
                        m_State = STATE_OBJ_STOPPED;
                        SetIcon(IDI_STEREO_GREEN);
                    }
                    TCHAR str[256];
                    int len = wsprintf(str, TEXT("Captured: (%d, %d, %d) %s"),
                        new_pos.x, new_pos.y, new_pos.z,
                        //delta.x, delta.y, delta.z, 
                        ((m_State == STATE_OBJ_STOPPED) ? L"STOPPED" : L"MOVING"));
                    SetStatus(str);
                }
                else
                {
                    SetStatus(L"Capturing...");
                    SetIcon(IDI_STEREO);
                }
            }
        }
        break;
    case WM_LBUTTONDOWN:
        {
            if ((mode == TRACKING_CALIBRATE_MODE) && (view_cam != MERGED_CAMERA_VIEW))
            {
                SetCapture(hWnd);
                int x = LOWORD(lParam), y = HIWORD(lParam);
                cv::Mat _HSV;
                cv::Mat _RGB = rawM(cv::Rect(x, y, 1, 1)); // use your x and y value
                cvtColor(_RGB, _HSV, cv::COLOR_BGR2HSV);
                cv::Vec3b hsv = _HSV.at<cv::Vec3b>(0, 0);
                h_min = hsv.val[0];
                s_min = hsv.val[1];
                v_min = hsv.val[2];
                UpdateHSVTrackbars();
                TCHAR str[256];
                int len = wsprintf(str, TEXT("HSV at (%d,%d) - (%d,%d,%d)"),
                    LOWORD(lParam), HIWORD(lParam), hsv.val[0], hsv.val[1], hsv.val[2]);
                SetStatus(str);
                ReleaseCapture();
            }
        }
    break;

    case WM_LBUTTONUP:
    {
        if ((mode == TRACKING_CALIBRATE_MODE) && (view_cam != MERGED_CAMERA_VIEW))
        {
            SetCapture(hWnd);
            int x = LOWORD(lParam), y = HIWORD(lParam);
            cv::Vec3b COLOR_MIN(h_min, s_min, v_min);// = (view_cam == LEFT_CAMERA_VIEW) ? COLOR_MIN_L : COLOR_MIN_R;
            cv::Vec3b COLOR_MAX(h_max, s_max, v_max);// = (view_cam == LEFT_CAMERA_VIEW) ? COLOR_MAX_L : COLOR_MAX_R;
            cv::Mat _HSV;
            cv::Mat _RGB = rawM(cv::Rect(x, y, 1, 1)); // use your x and y value
            cvtColor(_RGB, _HSV, cv::COLOR_BGR2HSV);
            cv::Vec3b hsv = _HSV.at<cv::Vec3b>(0, 0);
            for (int i = 0; i < 3; i++)
            {
                if (hsv.val[i] > COLOR_MIN[i])
                {
                    COLOR_MAX[i] = hsv.val[i];
                }
                else
                {
                    COLOR_MAX[i] = COLOR_MIN[i];
                    COLOR_MIN[i] = hsv.val[i];
                }
            }
            h_min = COLOR_MIN[0];
            h_max = COLOR_MAX[0];
            s_min = COLOR_MIN[1];
            s_max = COLOR_MAX[1];
            v_min = COLOR_MIN[2];
            v_max = COLOR_MAX[2];
            UpdateHSVTrackbars();
            GetColors(view_cam);

            //if (view_cam == RIGHT_CAMERA_VIEW)
            //{
            //    COLOR_MIN_R = COLOR_MIN;
            //    COLOR_MAX_R = COLOR_MAX;
            //}
            //else
            //{
            //    COLOR_MIN_L = COLOR_MIN;
            //    COLOR_MAX_L = COLOR_MAX;
            //}

            TCHAR str[256];
            int len = wsprintf(str, TEXT("HSV range: (%d,%d,%d) - (%d,%d,%d)"),
                h_min, s_min, v_min, h_max, s_max, v_max);
            SetStatus(str);
            ReleaseCapture();
        }
    }
    break;
    case WM_KEYDOWN:
    {
        switch (wParam)
        {
        case VK_SPACE:
            if (mode == MOTION_CALIBRATE_MODE)
            {
                TCHAR str[256];
                int len = wsprintf(str, TEXT("Real: (%d,%d,%d), Screen: (%d,%d,%d)"),
                        realX, realY, realZ,
                        cL.x, cL.y, (int)eu_dist(cL, cR));
                realPx.push_back(realX);
                imgPx.push_back(cL.x);
                realPy.push_back(realY);
                imgPy.push_back(cL.y);
                realPz.push_back(realZ);
                imgPz.push_back(eu_dist(cL, cR));
                SetStatus(str);
            }
            break;
        case VK_RETURN:
            if (mode == MOTION_CALIBRATE_MODE)
            {
                SetCoeff();
            }
            break;
        
        default:
            return DefWindowProc(hWnd, message, wParam, lParam);
        }

    }
    break;
    case WM_ERASEBKGND:
    {
        return 1;
    }
    break;
    //case WM_SIZE:
    //    if (wParam==SIZE_MINIMIZED)
    //        ShowWindow(hWnd, SW_HIDE);
    //    break;
    case WM_TRAYICON:
    {
        if (lParam == WM_LBUTTONDBLCLK)
            if (!IsWindowVisible(hWnd))
            {
                ShowWindow(hWnd, SW_SHOW);
                SetActiveWindow(hWnd);
            }

            else
                ShowWindow(hWnd, SW_HIDE);
    }
    break;
    default:
        return DefWindowProc(hWnd, message, wParam, lParam);
    }
    return 0;
}

// Message handler for about box.
INT_PTR CALLBACK About(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
{
    UNREFERENCED_PARAMETER(lParam);
    switch (message)
    {
    case WM_INITDIALOG:
        return (INT_PTR)TRUE;

    case WM_COMMAND:
        if (LOWORD(wParam) == IDOK || LOWORD(wParam) == IDCANCEL)
        {
            EndDialog(hDlg, LOWORD(wParam));
            return (INT_PTR)TRUE;
        }
        break;
    }
    return (INT_PTR)FALSE;
}

HRESULT InitializeCapture()
{
    if (m_State == STATE_INITIALISING)
    {
        if (!camL.isOpened())
            camL.open(camL_id);
        if (!camR.isOpened())
            camR.open(camR_id);
        if (camL.isOpened()&&camR.isOpened())
        {
            //camL.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
            //camL.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
            //camR.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
            //camR.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
            m_State = STATE_IDLE;
            RECT rc = { 0,0,(long)camL.get(cv::CAP_PROP_FRAME_WIDTH), (long)camL.get(cv::CAP_PROP_FRAME_HEIGHT) };
            AdjustWindowRect(&rc, GetWindowLong(hWnd, GWL_STYLE), !!GetMenu(hWnd));
            SetWindowPos(hWnd, 0, 0, 0, rc.right - rc.left, rc.bottom - rc.top, SWP_NOMOVE);
            StartCapturing();
            SetViewCam(MERGED_CAMERA_VIEW);
            SetTimer(hWnd, 1, 50, (TIMERPROC)NULL);
            return S_OK;
        }
        else
            return E_UNEXPECTED;
    }
    return E_UNEXPECTED;
}

void StartCapturing()
{
    SetMode(CAPTURE_MODE);
    SetViewCam(MERGED_CAMERA_VIEW);
    SetIcon(IDI_STEREO_GREEN);
}

void AbortCapturing()
{
    SetMode(STOP_MODE);
    SetStatus(L"Tracking is OFF");
    SetIcon(IDI_STEREO);
}

void CalibrateTracking()
{
    SetMode(TRACKING_CALIBRATE_MODE);
    SetViewCam(LEFT_CAMERA_VIEW);
    SetStatus(L"Tracking calibration");
    SetIcon(IDI_STEREO);
    cv::namedWindow(track_calibr_wnd);// , cv::WINDOW_NORMAL);
    cv::resizeWindow(track_calibr_wnd, 360, 640);
    cv::createTrackbar("dp*10:", track_calibr_wnd, &dp, 100);
    cv::createTrackbar("Kernel size:", track_calibr_wnd, &kernel_size, 10);
    cv::createTrackbar("Min dist:", track_calibr_wnd, &minDist, 500);
    cv::createTrackbar("Canny thr:", track_calibr_wnd, &p1, 500);
    cv::createTrackbar("Accum thr:", track_calibr_wnd, &p2, 500);
    cv::createTrackbar("Min radius:", track_calibr_wnd, &minR, 100);
    cv::createTrackbar("Max radius:", track_calibr_wnd, &maxR, 500);
    cv::createTrackbar("Alpha:", track_calibr_wnd, &alpha, 256);
    cv::createTrackbar("Beta:", track_calibr_wnd, &beta, 256);
    cv::createTrackbar("Hue min:", track_calibr_wnd, &h_min, 180);
    cv::createTrackbar("Hue max:", track_calibr_wnd, &h_max, 180);
    cv::createTrackbar("Sat min:", track_calibr_wnd, &s_min, 255);
    cv::createTrackbar("Sat max:", track_calibr_wnd, &s_max, 255);
    cv::createTrackbar("Val min:", track_calibr_wnd, &v_min, 255);
    cv::createTrackbar("Val max:", track_calibr_wnd, &v_max, 255);
}

void CalibrateMotion()
{
    SetMode(MOTION_CALIBRATE_MODE);
    SetViewCam(MERGED_CAMERA_VIEW);
    SetStatus(L"Tracking calibration");
    SetIcon(IDI_STEREO);
    cv::namedWindow(motion_calibr_wnd);// , cv::WINDOW_NORMAL);
    cv::resizeWindow(motion_calibr_wnd, 350, 150);
    cv::createTrackbar("Real X:", motion_calibr_wnd, &realX, max_realX);
    cv::createTrackbar("Real Y:", motion_calibr_wnd, &realY, max_realY);
    cv::createTrackbar("Real Z:", motion_calibr_wnd, &realZ, max_realZ);
    realPx.clear();
    realPy.clear();
    realPz.clear();
    imgPx.clear();
    imgPy.clear();
    imgPz.clear();
}

void SetViewCam(int id_view)
{
    GetColors(view_cam);
    view_cam = id_view;
    SetColors(view_cam);
    UpdateHSVTrackbars();
    CheckMenuItem(GetMenu(hWnd), IDM_VIEW_LEFTCAM, (id_view==LEFT_CAMERA_VIEW)?MF_CHECKED:MF_UNCHECKED);
    CheckMenuItem(GetMenu(hWnd), IDM_VIEW_RIGHTCAM, (id_view == RIGHT_CAMERA_VIEW) ? MF_CHECKED : MF_UNCHECKED);
    CheckMenuItem(GetMenu(hWnd), IDM_VIEW_MERGEDVIEW, (id_view == MERGED_CAMERA_VIEW) ? MF_CHECKED : MF_UNCHECKED);
}

void SetMode(int id_mode)
{
    mode = id_mode;
    CheckMenuItem(GetMenu(hWnd), IDM_OPERATE_RUN, (id_mode == CAPTURE_MODE) ? MF_CHECKED : MF_UNCHECKED);
    CheckMenuItem(GetMenu(hWnd), IDM_OPERATE_STOP, (id_mode == STOP_MODE) ? MF_CHECKED : MF_UNCHECKED);
    CheckMenuItem(GetMenu(hWnd), IDM_CALIBRATE_STEREO, (id_mode == STEREO_CALIBRATE_MODE) ? MF_CHECKED : MF_UNCHECKED);
    CheckMenuItem(GetMenu(hWnd), IDM_CALIBRATE_TRACKING, (id_mode == TRACKING_CALIBRATE_MODE) ? MF_CHECKED : MF_UNCHECKED);
    CheckMenuItem(GetMenu(hWnd), IDM_CALIBRATE_MOTION, (id_mode == MOTION_CALIBRATE_MODE) ? MF_CHECKED : MF_UNCHECKED);
    if ((id_mode != TRACKING_CALIBRATE_MODE) && (cv::getWindowProperty(track_calibr_wnd, cv::WND_PROP_VISIBLE) > 0))
        cv::destroyWindow(track_calibr_wnd);
    if ((id_mode != MOTION_CALIBRATE_MODE) && (cv::getWindowProperty(motion_calibr_wnd, cv::WND_PROP_VISIBLE) > 0))
        cv::destroyWindow(motion_calibr_wnd);

}

void SetStatus(const TCHAR* str)
{
    SetWindowText(hWnd, str);
}


void SetIcon(int icon_id)
{
    HICON hIcon = LoadIcon(hInst, MAKEINTRESOURCE(icon_id));
    SendMessage(hWnd, WM_SETICON, ICON_SMALL, (LPARAM)hIcon);
    SendMessage(hWnd, WM_SETICON, ICON_BIG, (LPARAM)hIcon);
    if (trayIcon)
    {
        nid.hIcon = LoadIcon(hInst, MAKEINTRESOURCE(icon_id));
        trayIcon = Shell_NotifyIcon(NIM_MODIFY, &nid);
    }
}
void SetColors(int view_cam_id)
{
    h_min = (view_cam_id == RIGHT_CAMERA_VIEW) ? COLOR_MIN_R[0] : COLOR_MIN_L[0];
    h_max = (view_cam_id == RIGHT_CAMERA_VIEW) ? COLOR_MAX_R[0] : COLOR_MAX_L[0];
    s_min = (view_cam_id == RIGHT_CAMERA_VIEW) ? COLOR_MIN_R[1] : COLOR_MIN_L[1];
    s_max = (view_cam_id == RIGHT_CAMERA_VIEW) ? COLOR_MAX_R[1] : COLOR_MAX_L[1];
    v_min = (view_cam_id == RIGHT_CAMERA_VIEW) ? COLOR_MIN_R[2] : COLOR_MIN_L[2];
    v_max = (view_cam_id == RIGHT_CAMERA_VIEW) ? COLOR_MAX_R[2] : COLOR_MAX_L[2];
}

void GetColors(int view_cam_id)
{
    if (view_cam_id == RIGHT_CAMERA_VIEW)
    {
        COLOR_MIN_R = cv::Vec3b(h_min, s_min, v_min);
        COLOR_MAX_R = cv::Vec3b(h_max, s_max, v_max);
    }
    else
    {
        COLOR_MIN_L = cv::Vec3b(h_min, s_min, v_min);
        COLOR_MAX_L = cv::Vec3b(h_max, s_max, v_max);
    }
}

void SendMsg(const char* str)
{
    DWORD dwBytesToWrite,dwBytesWritten;
    dwBytesToWrite = strlen(str) * sizeof(char);
    if (logFile != INVALID_HANDLE_VALUE)
        WriteFile(logFile, (LPVOID)str, dwBytesToWrite, &dwBytesWritten, NULL);
}

void UpdateHSVTrackbars()
{
    if (cv::getWindowProperty(track_calibr_wnd, cv::WND_PROP_VISIBLE) > 0)
    {
        cv::setTrackbarPos("Hue min:", track_calibr_wnd, h_min);
        cv::setTrackbarPos("Hue max:", track_calibr_wnd, h_max);
        cv::setTrackbarPos("Sat min:", track_calibr_wnd, s_min);
        cv::setTrackbarPos("Sat max:", track_calibr_wnd, s_max);
        cv::setTrackbarPos("Val min:", track_calibr_wnd, v_min);
        cv::setTrackbarPos("Val max:", track_calibr_wnd, v_max);
    }
}

HRESULT CaptureFrame()
{
    if (!camL.isOpened())
        camL.open(camL_id);
    if (!camR.isOpened())
        camR.open(camR_id);
    cL = cv::Point(); cR = cv::Point();
    if ((!camL.isOpened())&&(!camR.isOpened()))
        return E_UNEXPECTED;
    if ((m_State != STATE_INITIALISING) && (m_State != STATE_ERROR))
    {
        GetColors(view_cam);
        captured = false;
        camL.read(rawL);
        cv::normalize(rawL, rawL, alpha, beta, cv::NORM_MINMAX);
        cL = detectContours(rawL, COLOR_MIN_L, COLOR_MAX_L,0,0);
        camR.read(rawR);
        cv::normalize(rawR, rawR, alpha, beta, cv::NORM_MINMAX);
        cR = detectContours(rawR, COLOR_MIN_R, COLOR_MAX_R,0,0);
        captured = ((cL.x*cL.y != 0) && (cR.x*cR.y != 0));
        if (view_cam == MERGED_CAMERA_VIEW)
            cv::addWeighted(rawL, 0.5, rawR, 0.5, 0.0, rawM);
        else if (view_cam == RIGHT_CAMERA_VIEW)
            rawM = rawR;
        else
            rawM = rawL;
        //if (hWnd)
        RedrawWindow(hWnd, NULL, NULL, RDW_INVALIDATE );
        //InvalidateRect(hWnd, NULL, TRUE);
//            UpdateWindow(hWnd);
        return S_OK;
    }
    captured = false;
    return E_UNEXPECTED;
}

cv::Point detectCircles(cv::Mat input, cv::Vec3b min, cv::Vec3b max)
{
    cv::Mat img, hsv, mask, dst;
    cv::Point c;
    std::vector<cv::Vec3f> circles;
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    std::vector<cv::Point> approx;
    cv::Size k(kernel_size, kernel_size);
    cv::Moments M;
    cv::cvtColor(input, hsv, cv::COLOR_BGR2HSV);
    //cv::blur(hsv, hsv, k);
    cv::inRange(hsv, min, max, mask);
    //    cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7)),cv::Point(-1,-1),1);
    //    cv::morphologyEx(mask, mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7)),cv::Point(-1,-1),1);
    cv::dilate(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, k));
    cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, k));
    cv::blur(mask, mask, k);

    //cv::copyTo(input, img, mask);
    cv::bitwise_and(input, input, img, mask);
    //    cv::blur(img, img, cv::Size(7, 7));
    //input = img;
    cv::flip(img, input, 0);
    cv::flip(input, input, 0);
//    input = img.clone();
    cv::cvtColor(img, dst, cv::COLOR_BGR2GRAY);

    cv::HoughCircles(dst, circles, cv::HOUGH_GRADIENT,
            (double)dp / 10,    // This is the ratio of the resolution of original image to the accumulator matrix
            minDist,    // minimum distance between detected circles
            p1,    // Canny edge detection minVal - higher threshold
            p2,     // Canny edge detection maxVal
            minR,     // Minimum circle radius
            maxR     // Maximum circle radius
            // (min_radius & max_radius) to detect larger circles
        );

    for (int i = 0; i < circles.size(); i++)
    {
            //        cv::Vec3i c = circlesL[i];
        c = cv::Point(circles[i][0], circles[i][1]);
            // circle center
        cv::circle(input, c, 3, cv::Scalar(255, 255, 255), -1, cv::LINE_AA);
            // circle outline
        circle(input, c, circles[i][2], cv::Scalar(255, 255, 255), 2, cv::LINE_AA);
            // circle center pos
        cv::putText(input, cv::format("(%d,%d) %d", c.x, c.y, circles.size()), cv::Point(c.x-40,c.y-20), 1, 1, cv::Scalar(255, 255, 255), 2);
    }
        //cv::copyTo(dst, input, mask);
    if (circles.size() > 0)
        return cv::Point(circles[0][0], circles[0][1]);
    else
        return cv::Point();
}

cv::Point detectContours(cv::Mat input, cv::Vec3b min, cv::Vec3b max, int type = 0, int view = 0)
{
    cv::Mat img, hsv, mask, dst;
    cv::Point c(0,0);
    std::vector<cv::Vec3f> circles;
    std::vector<std::vector<cv::Point>> contours;

    std::vector<cv::Vec4i> hierarchy;
    std::vector<cv::Point> approx;
    cv::Moments M;
    if (kernel_size == 0)
        kernel_size = 1;
    if (p1 == 0)
        p1 = 1;
    if (p2 == 0)
        p2 = 1;
    cv::Size k(kernel_size, kernel_size);
    cv::cvtColor(input, hsv, cv::COLOR_BGR2HSV);
    double img_area = double(input.rows * input.cols);
    cv::blur(hsv, hsv, k);
    cv::inRange(hsv, min, max, mask);
    //    cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7)),cv::Point(-1,-1),1);
    //    cv::morphologyEx(mask, mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7)),cv::Point(-1,-1),1);

    cv::dilate(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, k));
    cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, k));
//  cv::blur(mask, mask, k);

    cv::bitwise_and(input, input, img, mask);
//  cv::blur(img, img, cv::Size(7, 7));
    if (view > 0)
    {
        cv::flip(img, input, 0);
        cv::flip(input, input, 0);
    }
    cv::cvtColor(img, dst, cv::COLOR_BGR2GRAY);
    if (type == 0) // contours
    {
        cv::findContours(dst, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
        // sorting contours from largest to smallest
        //std::sort(contours.begin(), contours.end(), compareContourAreas);

        // extracting the largest contour
        // std::vector<cv::Point> cnt = contours[0];
        for (int i = 0; i < contours.size(); i++)
        {

            if (fabs(cv::contourArea(cv::Mat(contours[i]))) > img_area * 0.0001)
            {
                cv::drawContours(input, contours, i, cv::Scalar(255, 255, 255), 2, 8);// , hierarchy, 1);
                cv::approxPolyDP(contours[i], approx, cv::arcLength(contours[i], true) * 0.01, true);
                M = cv::moments(approx);
                c = cv::Point(int(M.m10 / M.m00), int(M.m01 / M.m00));
                cv::polylines(input, approx, true, cv::Scalar(255, 255, 255));
                cv::circle(input, c, 3, cv::Scalar(255, 255, 255), -1);
                //        }
                //if (contours.size() > 0)
                return c;
            }
        }
    }
    else // circles
    {
        cv::HoughCircles(dst, circles, cv::HOUGH_GRADIENT,
            (double)dp / 10,    // This is the ratio of the resolution of original image to the accumulator matrix
            minDist,    // minimum distance between detected circles
            p1,    // Canny edge detection minVal - higher threshold
            p2,     // Canny edge detection maxVal
            minR,     // Minimum circle radius
            maxR     // Maximum circle radius
            // (min_radius & max_radius) to detect larger circles
        );

        for (int i = 0; i < circles.size(); i++)
        {
            //        cv::Vec3i c = circlesL[i];
            c = cv::Point(circles[i][0], circles[i][1]);
            // circle center
            cv::circle(input, c, 3, cv::Scalar(255, 255, 255), -1, cv::LINE_AA);
            // circle outline
            circle(input, c, circles[i][2], cv::Scalar(255, 255, 255), 2, cv::LINE_AA);
            // circle center pos
            cv::putText(input, cv::format("(%d,%d) %d", c.x, c.y, circles.size()), cv::Point(c.x - 40, c.y - 20), 1, 1, cv::Scalar(255, 255, 255), 2);
        }
        //cv::copyTo(dst, input, mask);
        if (circles.size() > 0)
            return cv::Point(circles[0][0], circles[0][1]);
    }
    return cv::Point(0, 0);
}